
// import oimo.dynamics.TimeStep;
// import oimo.m.Mat3;
// import oimo.m.Quat;
// import oimo.m.Vec3;
// import oimo.m.M;
// import oimo.common.MathUtil;
// import oimo.common.Vec3;
// import oimo.dynamics.*;
// import oimo.dynamics.constraint.info.JacobianRow;
// import oimo.dynamics.constraint.info.joint.JointSolverInfo;
// import oimo.dynamics.constraint.info.joint.JointSolverInfoRow;

import { Joint } from "./Joint";
import { SpringDamper } from "./SpringDamper";
import { TranslationalLimitMotor } from "./TranslationalLimitMotor";
import { JointBasis } from "./JointBasis";
import { Vec3 } from "../../../common/Vec3";

import { JointType } from "./JointType";
import { M } from "../../../common/M";
import { JointSolverInfo } from "../info/joint/JointSolverInfo";
import { TimeStep } from "../../TimeStep";
import { Mat3 } from "../../../common/Mat3";
import { JointSolverInfoRow } from "../info/joint/JointSolverInfoRow";
import { JacobianRow } from "../info/JacobianRow";
import { PrismaticJointConfig } from "./PrismaticJointConfig";
import { Quat } from "../../../common/Quat";
import { MathUtil } from "../../../common/MathUtil";

/**
 * A prismatic joint (a.k.a. slider joint) constrains two rigid bodies to
 * share their anchor points and constraint axes, and restricts relative
 * translation onto the constraint axis. This joint provides one degree of
 * freedom. You can enable lower and upper limits, a motor, a spring and
 * damper effect of the translational part of the constraint.
 */

export class PrismaticJoint extends Joint {
	_sd: SpringDamper;
	_lm: TranslationalLimitMotor;

	_basis: JointBasis;

	translation: number;
	linearErrorY: number;
	linearErrorZ: number;
	angularError: Vec3;

	/**
	 * Creates a new prismatic joint by configuration `config`.
	 */
	constructor(config: PrismaticJointConfig) {
		super(config, JointType.PRISMATIC);

		M.vec3_fromVec3(this._localBasisX1, config.localAxis1);
		M.vec3_fromVec3(this._localBasisX2, config.localAxis2);

		this.buildLocalBasesFromX();

		this._basis = new JointBasis(this);

		this.translation = 0;
		this.linearErrorY = 0;
		this.linearErrorZ = 0;
		M.vec3_zero(this.angularError);

		this._sd = config.springDamper.clone();
		this._lm = config.limitMotor.clone();
	}

	// --- priate ---


	public getInfo(info: JointSolverInfo, timeStep: TimeStep, isPositionPart: boolean): void {
		// compute ERP
		var erp: number = this.getErp(timeStep, isPositionPart);

		// compute rhs
		var linRhsY: number = this.linearErrorY * erp;
		var linRhsZ: number = this.linearErrorZ * erp;
		var angRhsX: number = M.vec3_get(this.angularError, 0) * erp;
		var angRhsY: number = M.vec3_get(this.angularError, 1) * erp;
		var angRhsZ: number = M.vec3_get(this.angularError, 2) * erp;

		var crossR1: Mat3 = new Mat3();
		var crossR2: Mat3 = new Mat3();
		M.vec3_toCrossMatrix(crossR1, this._relativeAnchor1);
		M.vec3_toCrossMatrix(crossR2, this._relativeAnchor2);
		M.mat3_negate(crossR1, crossR1);
		M.mat3_negate(crossR2, crossR2);

		var row: JointSolverInfoRow;
		var j: JacobianRow;
		var motorMass: number = 1 / (this._b1._invMass + this._b2._invMass);

		// linear X
		if (this._sd.frequency <= 0 || !isPositionPart) {
			row = info.addRow(this._impulses[0]);
			this.setSolverInfoRowLinear(row, this.translation, this._lm, motorMass, this._sd, timeStep, isPositionPart);

			j = row.jacobian;
			M.vec3_assign(j.lin1, this._basis.x);
			M.vec3_assign(j.lin2, this._basis.x);
			M.mat3_getRow(j.ang1, crossR1, 0);
			M.mat3_getRow(j.ang2, crossR2, 0);
		}

		// linear Y
		row = info.addRow(this._impulses[1]);
		row.equalLimit(linRhsY, 0);

		j = row.jacobian;
		M.vec3_assign(j.lin1, this._basis.y);
		M.vec3_assign(j.lin2, this._basis.y);
		M.mat3_getRow(j.ang1, crossR1, 1);
		M.mat3_getRow(j.ang2, crossR2, 1);

		// linear Z
		row = info.addRow(this._impulses[2]);
		row.equalLimit(linRhsZ, 0);

		j = row.jacobian;
		M.vec3_assign(j.lin1, this._basis.z);
		M.vec3_assign(j.lin2, this._basis.z);
		M.mat3_getRow(j.ang1, crossR1, 2);
		M.mat3_getRow(j.ang2, crossR2, 2);

		// angular X
		row = info.addRow(this._impulses[3]);
		row.equalLimit(angRhsX, 0);

		j = row.jacobian;
		M.vec3_set(j.ang1, 1, 0, 0);
		M.vec3_set(j.ang2, 1, 0, 0);

		// angular Y
		row = info.addRow(this._impulses[4]);
		row.equalLimit(angRhsY, 0);

		j = row.jacobian;
		M.vec3_set(j.ang1, 0, 1, 0);
		M.vec3_set(j.ang2, 0, 1, 0);

		// angular Z
		row = info.addRow(this._impulses[5]);
		row.equalLimit(angRhsZ, 0);

		j = row.jacobian;
		M.vec3_set(j.ang1, 0, 0, 1);
		M.vec3_set(j.ang2, 0, 0, 1);
	}


	private computeErrors(): void {
		// compute angular error
		var rot1: Mat3 = new Mat3();
		var rot2: Mat3 = new Mat3();
		M.mat3_fromCols(rot1, this._basisX1, this._basisY1, this._basisZ1);
		M.mat3_fromCols(rot2, this._basisX2, this._basisY2, this._basisZ2);
		var relRot: Mat3 = new Mat3();
		M.mat3_mulRhsTransposed(relRot, rot2, rot1);
		var relQ: Quat = new Quat();
		M.quat_fromMat3(relQ, relRot);

		var cosHalfTheta: number = M.quat_getReal(relQ);
		var theta: number = MathUtil.safeAcos(cosHalfTheta) * 2;
		// [rotation vector] = [rotation axis] * [rotation angle]
		M.vec3_fromQuat(this.angularError, relQ);
		M.vec3_normalize(this.angularError, this.angularError);
		M.vec3_scale(this.angularError, this.angularError, theta);

		// compute linear error
		var anchorDiff: Vec3 = new Vec3();
		M.vec3_sub(anchorDiff, this._anchor2, this._anchor1);
		this.translation = M.vec3_dot(anchorDiff, this._basis.x);
		this.linearErrorY = M.vec3_dot(anchorDiff, this._basis.y);
		this.linearErrorZ = M.vec3_dot(anchorDiff, this._basis.z);
	}

	// --- internal ---

	public _syncAnchors(): void {
		super._syncAnchors();
		this._basis.trackByX();

		// compute positional errors
		this.computeErrors();
	}

	public _getVelocitySolverInfo(timeStep: TimeStep, info: JointSolverInfo): void {
		super._getVelocitySolverInfo(timeStep, info);
		this.getInfo(info, timeStep, false);
	}

	public _getPositionSolverInfo(info: JointSolverInfo): void {
		super._getPositionSolverInfo(info);
		this.getInfo(info, null, true);
	}

	// --- public ---

	/**
	 * Returns the first rigid body's constraint axis in world coordinates.
	 */
	public getAxis1(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._basisX1);
		return v;
	}

	/**
	 * Returns the second rigid body's constraint axis in world coordinates.
	 */
	public getAxis2(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._basisX2);
		return v;
	}

	/**
	 * Sets `axis` to the first rigid body's constraint axis in world coordinates.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getAxis1To(axis: Vec3): void {
		M.vec3_toVec3(axis, this._basisX1);
	}

	/**
	 * Sets `axis` to the second rigid body's constraint axis in world coordinates.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getAxis2To(axis: Vec3): void {
		M.vec3_toVec3(axis, this._basisX2);
	}

	/**
	 * Returns the first rigid body's constraint axis relative to the rigid body's transform.
	 */
	public getLocalAxis1(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._localBasisX1);
		return v;
	}

	/**
	 * Returns the second rigid body's constraint axis relative to the rigid body's transform.
	 */
	public getLocalAxis2(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._localBasisX2);
		return v;
	}

	/**
	 * Sets `axis` to the first rigid body's constraint axis relative to the rigid body's transform.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getLocalAxis1To(axis: Vec3): void {
		M.vec3_toVec3(axis, this._localBasisX1);
	}

	/**
	 * Sets `axis` to the second rigid body's constraint axis relative to the rigid body's transform.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getLocalAxis2To(axis: Vec3): void {
		M.vec3_toVec3(axis, this._localBasisX2);
	}

	/**
	 * Returns the translational spring and damper settings.
	 */
	public getSpringDamper(): SpringDamper {
		return this._sd;
	}

	/**
	 * Returns the translational limits and motor settings.
	 */
	public getLimitMotor(): TranslationalLimitMotor {
		return this._lm;
	}

	/**
	 * Returns the translation of the joint.
	 */
	public getTranslation(): number {
		return this.translation;
	}

}
